import math
from shapely.geometry import Polygon
from shapely.ops import unary_union
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon as MplPolygon


def calculate_boundary_points(path_points, width):
    """
    根据农机作业路径点计算左边界点和右边界点数组。

    参数:
    path_points: list of list, 每个元素为 [经度, 纬度, 朝向角(弧度)]
    width: float, 农机作业宽度（米）

    返回:
    left_points: list of list, 左边界点数组 [[经度, 纬度], ...]
    right_points: list of list, 右边界点数组 [[经度, 纬度], ...]
    """
    left_points = []
    right_points = []

    for point in path_points:
        lon, lat, angle = point
        lat_rad = math.radians(lat)  # 纬度转换为弧度

        # 计算左边界移动向量（米）
        east_left = -math.cos(angle) * width / 2
        north_left = math.sin(angle) * width / 2

        # 计算右边界移动向量（米）
        east_right = math.cos(angle) * width / 2
        north_right = -math.sin(angle) * width / 2

        # 计算经纬度变化量（度）
        delta_lon_left = east_left / (111000 * math.cos(lat_rad))
        delta_lat_left = north_left / 111000
        delta_lon_right = east_right / (111000 * math.cos(lat_rad))
        delta_lat_right = north_right / 111000

        # 计算左右边界点
        left_point = [lon + delta_lon_left, lat + delta_lat_left]
        right_point = [lon + delta_lon_right, lat + delta_lat_right]

        left_points.append(left_point)
        right_points.append(right_point)

    return left_points, right_points


def calculate_unworked_areas(left_points, right_points, field_boundary):
    """
    根据左边界点数组、右边界点数组和农田边界计算未作业区域。

    参数:
    left_points: list of list, 左边界点数组 [[经度, 纬度], ...]
    right_points: list of list, 右边界点数组 [[经度, 纬度], ...]
    field_boundary: list of list, 农田多边形边界点 [[经度, 纬度], ...]

    返回:
    unworked_poly: 未作业区域的Shapely多边形对象
    coverage_poly: 作业区域的Shapely多边形对象
    """
    # 创建农田多边形
    field_poly = Polygon(field_boundary)

    # 检查左右点数组长度是否一致
    n = len(left_points)
    if n != len(right_points):
        raise ValueError("左边界点数组和右边界点数组长度不一致")

    # 构建作业带多边形列表（由四边形组成）
    polygons_list = []
    for i in range(n - 1):
        # 获取当前点和下一个点的左右边界点
        left_i = left_points[i]
        left_i1 = left_points[i + 1]
        right_i = right_points[i]
        right_i1 = right_points[i + 1]

        # 构建四边形多边形（点顺序为顺时针或逆时针一致）
        poly = Polygon([left_i, left_i1, right_i1, right_i])
        polygons_list.append(poly)

    # 合并所有四边形多边形
    coverage_poly = unary_union(polygons_list)

    # 计算未作业区域：农田多边形减去作业带多边形
    unworked_poly = field_poly.difference(coverage_poly)

    return unworked_poly, coverage_poly


def plot_unworked_areas(unworked_poly, coverage_poly, field_boundary, left_points, right_points, path_points):
    """
    绘制农田、作业区域和未作业区域，并将未作业区域填充为红色。

    参数:
    unworked_poly: 未作业区域的Shapely多边形对象
    coverage_poly: 作业区域的Shapely多边形对象
    field_boundary: 农田边界点列表
    left_points: 左边界点数组
    right_points: 右边界点数组
    path_points: 农机路径点数组
    """
    # 创建图形
    fig, ax = plt.subplots(figsize=(10, 10))

    # 绘制农田边界
    field_patch = MplPolygon(field_boundary, closed=True, fill=False, edgecolor='black', linewidth=2)
    ax.add_patch(field_patch)

    # 绘制作业区域
    if coverage_poly.geom_type == 'MultiPolygon':
        for poly in coverage_poly.geoms:
            x, y = poly.exterior.xy
            patch = MplPolygon(list(zip(x, y)), closed=True, fill=True, edgecolor='green', facecolor='lightgreen',
                               alpha=0.7)
            ax.add_patch(patch)
    else:
        x, y = coverage_poly.exterior.xy
        patch = MplPolygon(list(zip(x, y)), closed=True, fill=True, edgecolor='green', facecolor='lightgreen',
                           alpha=0.7)
        ax.add_patch(patch)

    # 绘制未作业区域并填充为红色
    if unworked_poly.geom_type == 'MultiPolygon':
        for poly in unworked_poly.geoms:
            x, y = poly.exterior.xy
            patch = MplPolygon(list(zip(x, y)), closed=True, fill=True, edgecolor='red', facecolor='red', alpha=0.7)
            ax.add_patch(patch)

            # 如果有洞（内边界），也绘制出来
            for interior in poly.interiors:
                x, y = interior.xy
                patch = MplPolygon(list(zip(x, y)), closed=True, fill=True, edgecolor='red', facecolor='red', alpha=0.7)
                ax.add_patch(patch)
    else:
        x, y = unworked_poly.exterior.xy
        patch = MplPolygon(list(zip(x, y)), closed=True, fill=True, edgecolor='red', facecolor='red', alpha=0.7)
        ax.add_patch(patch)

        # 如果有洞（内边界），也绘制出来
        for interior in unworked_poly.interiors:
            x, y = interior.xy
            patch = MplPolygon(list(zip(x, y)), closed=True, fill=True, edgecolor='red', facecolor='red', alpha=0.7)
            ax.add_patch(patch)

    # 绘制农机路径点和边界线
    path_lons = [p[0] for p in path_points]
    path_lats = [p[1] for p in path_points]
    ax.plot(path_lons, path_lats, 'bo-', linewidth=2, markersize=6)

    left_lons = [p[0] for p in left_points]
    left_lats = [p[1] for p in left_points]
    ax.plot(left_lons, left_lats, 'g--', linewidth=1)

    right_lons = [p[0] for p in right_points]
    right_lats = [p[1] for p in right_points]
    ax.plot(right_lons, right_lats, 'g--', linewidth=1)

    # 设置图形属性
    ax.set_aspect('equal')
    ax.set_xlabel('经度')
    ax.set_ylabel('纬度')
    ax.set_title('农田作业区域分析（红色为未作业区域）')

    # 显示图形
    plt.tight_layout()
    plt.show()


# 示例使用
if __name__ == '__main__':
    # 示例路径点：假设有三个点，每个点 [经度, 纬度, 朝向角(弧度)]
    path_points = [
        [116.0, 39.0, 0],  # 北朝向
        [116.001, 39.001, math.pi / 2],  # 东朝向
        [116.002, 39.002, math.pi]  # 南朝向
    ]
    width = 10.0  # 作业宽度10米

    # 计算边界点
    left_points, right_points = calculate_boundary_points(path_points, width)

    # 示例农田边界（假设为一个矩形）
    field_boundary = [
        [115.999, 38.999],
        [116.003, 38.999],
        [116.003, 39.003],
        [115.999, 39.003],
        [115.999, 38.999]  # 闭合多边形
    ]

    # 计算未作业区域
    unworked_poly, coverage_poly = calculate_unworked_areas(left_points, right_points, field_boundary)

    # 绘制图形并将未作业区域填充为红色
    plot_unworked_areas(unworked_poly, coverage_poly, field_boundary, left_points, right_points, path_points)